#!/usr/bin/python3
# -*- coding: utf-8 -*-

"""
手柄控制模块
用于通过游戏手柄控制六足机器人和摄像头云台
"""

import time
import sys
import os
import logging
from typing import Dict, Optional

import inputs  # 轻量级输入设备控制库

from robot_core.actions import ActionManager
from robot_core.motion_controller import motion_controller
from robot_core.protocols.serial_protocol import SerialProtocol

# 配置日志
logging.basicConfig(
    level=logging.INFO, format="%(asctime)s - %(name)s - %(levelname)s - %(message)s"
)
logger = logging.getLogger(__name__)


class JoystickController:
    """手柄控制器类"""

    def __init__(self):
        """初始化手柄控制器"""
        self.running = True
        self.camera_y = 1500  # 垂直方向舵机中位值
        self.camera_x = 1500  # 水平方向舵机中位值

        # 初始化串口协议
        self.serial_protocol = SerialProtocol()

        # 初始化动作管理器
        self.action_manager = ActionManager()

        # 移动方向与动作映射表
        self.dir_action: Dict[str, str] = {
            "idle": "25",  # 待机姿势
            "front": "26",  # 前进
            "back": "27",  # 后退
            "left": "28",  # 左移
            "right": "29",  # 右移
        }

        # 摄像头方向控制参数
        self.camera_action: Dict[str, int] = {
            "idle": 0,  # 静止
            "up": -3,  # 向上
            "down": 3,  # 向下
            "left": -3,  # 左转
            "right": 3,  # 右转
        }

        # 按钮映射表
        self.button_actions: Dict[str, str] = {
            "BTN_SOUTH": "41",  # A键：打招呼
            "BTN_EAST": "40",  # B键：待机
            "BTN_NORTH": "9",  # Y键：跳舞
            "BTN_WEST": "0",  # X键：复位
        }

    def run_action(self, direction: str) -> None:
        """执行移动动作

        Args:
            direction: 移动方向，对应dir_action的键值
        """
        action_type = self.dir_action.get(direction, "0")
        action = self.action_manager.get_action(action_type)
        if action:
            motion_controller.execute_command(3, [action_type, "1"])  # 执行动作组

    def camera_turn(self, direction: str) -> None:
        """控制摄像头云台转动

        Args:
            direction: 转动方向，对应camera_action的键值
        """
        action_type = self.camera_action.get(direction, "0")

        if direction in ["up", "down"]:
            self.camera_y += 40 if direction == "up" else -40
            self.serial_protocol.write_command(
                1, 1, self.camera_y, 100
            )  # 设置舵机1位置
        elif direction in ["left", "right"]:
            self.camera_x += 40 if direction == "left" else -40
            self.serial_protocol.write_command(
                2, 1, self.camera_x, 100
            )  # 设置舵机2位置
        else:
            self.camera_y = 1500
            self.camera_x = 1500
            motion_controller._init_camera()  # 复位摄像头位置

    def handle_axis_event(self, event) -> None:
        """处理摇杆事件

        Args:
            event: inputs库的摇杆事件
        """
        try:
            # 左摇杆控制移动
            if event.code == "ABS_X":  # 左右方向
                if event.state < -20000:
                    self.run_action("left")
                elif event.state > 20000:
                    self.run_action("right")
                else:
                    self.run_action("idle")
            elif event.code == "ABS_Y":  # 前后方向
                if event.state < -20000:
                    self.run_action("front")
                elif event.state > 20000:
                    self.run_action("back")
                else:
                    self.run_action("idle")

            # 右摇杆控制摄像头
            elif event.code == "ABS_RX":  # 摄像头左右
                if event.state < -20000:
                    self.camera_turn("left")
                elif event.state > 20000:
                    self.camera_turn("right")
                else:
                    self.camera_turn("idle")
            elif event.code == "ABS_RY":  # 摄像头上下
                if event.state < -20000:
                    self.camera_turn("up")
                elif event.state > 20000:
                    self.camera_turn("down")
                else:
                    self.camera_turn("idle")
        except Exception as e:
            logger.error(f"处理摇杆事件时出错: {e}")

    def handle_button_event(self, event) -> None:
        """处理按钮事件

        Args:
            event: inputs库的按钮事件
        """
        try:
            if event.state == 1:  # 按钮按下
                action = self.button_actions.get(event.code)
                if action:
                    motion_controller.execute_command(3, [action, "1"])  # 执行动作组
        except Exception as e:
            logger.error(f"处理按钮事件时出错: {e}")

    def start(self) -> None:
        """启动手柄监听主循环"""
        logger.info("启动手柄控制器...")

        try:
            while self.running:
                events = inputs.get_gamepad()
                for event in events:
                    if event.ev_type == "Absolute":
                        self.handle_axis_event(event)
                    elif event.ev_type == "Key":
                        self.handle_button_event(event)

                time.sleep(0.01)  # 控制循环频率

        except KeyboardInterrupt:
            logger.info("收到退出信号，正在关闭...")
        except Exception as e:
            logger.error(f"运行时发生错误: {e}")
        finally:
            self.cleanup()

    def cleanup(self) -> None:
        """清理资源"""
        try:
            self.running = False
            motion_controller._init_camera()  # 复位摄像头位置
            self.serial_protocol.close()  # 关闭串口连接
            logger.info("手柄控制器已关闭")
        except Exception as e:
            logger.error(f"清理资源时出错: {e}")
        finally:
            os._exit(0)


def main():
    """主函数"""
    try:
        motion_controller._init_servos()  # 初始化舵机
        motion_controller._init_camera()  # 初始化摄像头
        controller = JoystickController()
        controller.start()
    except Exception as e:
        logger.error(f"程序运行出错: {e}")
        os._exit(1)


if __name__ == "__main__":
    main()
